#include <ros/ros.h>
#include <std_msgs/String.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/PointCloud.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Pose.h>
#include "robina_human_tracker_lrf/angle_tracker.h"
#include "robina_human_tracker_lrf/humanTorso.h"
#include <actionlib/client/simple_action_client.h>
#include <robina_following/followMe.h>
#include <robina_following/turnRobot.h>
#include <robina_following/turnRobotMsg.h>
#include <ros/service_server.h>
#include <nav_msgs/Odometry.h>
#include <tf/transform_listener.h>
#include <tf/tf.h>

#define MIN_DISTANCE                0.90
#define CRITIICAL_DISTANCE          0.60

#define FRONT_SONAR_MIN_DISTANCE    0.50
#define BACK_SONAR_MIN_DISTANCE    -0.80

#define turnRate_threshold          0.8

#define LAST_POSX_THRESHOLD         0.20
#define LAST_POSY_THRESHOLD         0.20

using namespace std;

class following
{

public:

    following();

    void setup();

private:
    ros::NodeHandle nh;

    ros::Publisher      cmdPub;
    ros::Subscriber     follow_cmd;
    ros::Subscriber     sonar_sub;
    ros::ServiceServer  followMe_srv_server;
    ros::ServiceServer  turnRobot_srv;

    geometry_msgs::Twist base_cmd;

    geometry_msgs::Pose2D humanLastPos;

    bool obstaclExist;
    bool isFollowEnabled;
    bool backObstacleExist;

    void sonarCb                    ( const sensor_msgs::PointCloudConstPtr &msg );
    void follow_operator            ( const robina_human_tracker_lrf::humanTorsoConstPtr &msg );

    void turnRobot                  ( double turnSideAngle);

    bool followMe_serviceCB         ( robina_following::followMe::Request &req,
                                      robina_following::followMe::Response &res );

    bool turnRobot_serviceCB        ( robina_following::turnRobot::Request &req,
                                      robina_following::turnRobot::Response &res);

    bool isHumanMoved               ( const robina_human_tracker_lrf::humanTorsoConstPtr &msg);

};
